/***************************************************************
|			MAIN
|
|This File is the main of the NXBOT proyect, intented to
|configure the firmware of the NXBOT Robots.
|
|There are five tasks to be executed:
|
****************************************************************/

#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdint.h>

#include "FreeRTOS.h"
#include "task.h"


#include "nxbot.h"


//handles for each task. Declared as global variables, in order to use them in other C modules.
xTaskHandle commHandle;
xTaskHandle soundHandle;
xTaskHandle sensorHandle;

int main( void )
{

	//default initialization of hardware
	nxbot_init();
	
	
	
	/* definition of global parameters for tasks. The sructure is allocated in memory
	 * A simple definition for globals: All parameters that are shared by all tasks.
	 * Although not all parameters are needed for each task, it is a simple way to share information between tasks.
	 * See include/structures.h for more detailed information.
	 */
	 
	general *globals;
	globals=(general *)pvPortMalloc( sizeof( general ) );

	/* Frequencies of Tasks, Which are measured in ticks.  
	 * Tick frequency is 3000 Hz, it means each tick last for 0.5ms.
	 * See FreeRTOSConfig.h, configTICKRATE_HZ variable for more detailed information.
	 */
	globals->gTimes.intervalMotors=MOTOR_INTERVAL; //250ms
	globals->gTimes.intervalComm=COMM_INTERVAL;
	globals->gTimes.intervalSensors=SENSOR_INTERVAL;

	//Initial Velocity of the motors
	globals->gVels.velLeftRef = 0;
	globals->gVels.velRightRef = 0;
	globals->gPiParam.Kp = 20;
	globals->gPiParam.Ki = 20;
	
	//Initial Position of the robot
	globals->gRobotState.posXAct = 0;
	globals->gRobotState.posYAct = 0;

	//now execute tasks
	executeSensorsTask(globals);
	executeCommTask(globals);
	executeMotorsTask(globals);
	executeSoundTask(globals);

	//suspend sound task. It will be enabled by comm task
	vTaskSuspend( soundHandle );

	// Start the scheduler for the tasks	
	vTaskStartScheduler();
	
	return 0;
}
